#include <stdio.h>
#include "hal_tcpc_operation.h"
#include "insdrv_i2c.h"
#include "pd_state_machine.h"
#include "typec_state_machine.h"

#define CHARGER_SLAVE_ADDR  (0x51 << 1)
#define TCPC_SLAVE_ADDR     (0x52 << 1)

static int _i2c_read_reg(uint8_t reg, uint8_t len, uint8_t *dst, uint8_t addr)
{
    int ret = 0, count = 5;
    uint8_t buffer = 0;

    while (count) {
        if (len > 1) {
            ret = _i2c1_rcv(addr, reg, dst, len);
            if (ret != 0)
                count--;
            else
                return ret;
        }
        else {
            ret = _i2c1_rcv(addr, reg, &buffer, 1);
            if (ret != 0)
                count--;
        else {
                *(uint8_t *)dst = buffer;
                return ret;
            }
        }
        delay_1us(500);
    }

    return ret;
}

static int _i2c_write_reg(uint8_t reg, uint8_t len, uint8_t *src, uint8_t addr)
{
    int ret = 0, count = 5;

    while (count) {
        if (len > 1) {
            ret = _i2c1_send(addr, reg, src, len);
            if (ret != 0)
                count--;
            else
                return ret;
        }
        else {
            ret = _i2c1_send(addr, reg, src, 1);
            if (ret != 0)
                count--;
            else
                return ret;
        }
        delay_1us(500);
    }

    return ret;
}

static int __attribute__((unused)) charger_i2c_read_reg(uint8_t reg, uint8_t len, uint8_t *dst)
{
    return _i2c_read_reg(reg, len, dst, CHARGER_SLAVE_ADDR);
}

static int charger_i2c_write_reg(uint8_t reg, uint8_t len, uint8_t *src)
{
    return _i2c_write_reg(reg, len, src, CHARGER_SLAVE_ADDR);
}

static int charger_i2c_write8(uint8_t reg, uint8_t data)
{
    int ret;

    ret =  charger_i2c_write_reg(reg, 1, &data);
    if (ret) {
        printf("charger i2c write8 Failed: reg=%02X, ret=%d\n", reg, ret);
        return ret;
    }

    return 0;
}

static int charger_i2c_read8(uint8_t reg, uint8_t *data)
{
    int ret;

    ret = charger_i2c_read_reg(reg, 1, data);
    if (ret) {
        printf("charger i2c read8 Failed: reg=%02X, ret=%d\n", reg, ret);
        return ret;
    }

    return 0;
}

static int charger_update_bits(uint8_t reg, uint8_t mask, uint8_t val)
{
    uint8_t tmp;
    int ret;

    ret = charger_i2c_read8(reg, &tmp);
    if (ret)
        return ret;

    tmp &= ~mask;
    tmp |= val & mask;
    return charger_i2c_write8(reg, tmp);
}

int charger_vbus_control(bool enable)
{
    int ret;

    if (enable) {
        ret = charger_update_bits(0x07, BIT(5) | BIT(4), BIT(5) | BIT(4));
        ret = charger_update_bits(0x3C, BIT(7) | BIT(2) | BIT(1), BIT(2) | BIT(1));
        if (ret)
            return ret;
    } else {
        ret = charger_update_bits(0x07, BIT(5) | BIT(4), 0);
        ret = charger_update_bits(0x3C, BIT(7) | BIT(2) | BIT(1), 0);
        if (ret)
            return ret;
    }

    return 0;
}

int charger_vbus_discharge(bool enable)
{
    return charger_update_bits(0x08, BIT(5), enable ? BIT(5) : 0);
}

int charger_set_otg_voltage(uint16_t mv)
{
    uint8_t vboost;

    if (mv <= 3900) {
        vboost = 0;
    } else if (mv >= 5800) {
        vboost = 0x13;
    } else {
        mv -= 3900;
        vboost = mv / 100;
    }

    return charger_update_bits(0x39, GENMASK(7, 3), vboost << 3);
}

int charger_set_otg_ilmt(uint16_t ma)
{
    uint8_t iboost_lim;

    if (ma <= 500) {
        iboost_lim = 0;
    } else if (ma >= 3250) {
        iboost_lim = 7;
    } else {
        ma -= 500;
        iboost_lim = (ma + 399) / 400;
    }

    return charger_update_bits(0x39, GENMASK(2, 0), iboost_lim << 0);
}

int charger_set_iindpm(uint16_t ma)
{
    uint8_t value;
    static bool is_hiz;

    if (ma < 100) {
        charger_set_hiz(true);
        is_hiz = true;
    } else {
        value = (ma - 100) / 50;
        if (is_hiz) {
            charger_set_hiz(false);
        }
    }

    return charger_update_bits(0x34, GENMASK(5, 0), value << 0);
}

int charger_set_hiz(bool enable)
{
    return charger_update_bits(0x3C, BIT(7), enable ? BIT(7) : 0);
}

int charger_hw_init(void)
{
//    uint8_t reg_val;

    // Disable Watchdog
    charger_i2c_write8(0x7, 0x0);

    // ICHG_CC 3A
    charger_i2c_write8(0x32, 0x3C);

    // IINDPM 3.25A
    charger_set_iindpm(3250);

    // Boost Mode Limit 5.1V/3.25A
    charger_set_otg_voltage(5100);
    charger_set_otg_ilmt(3250);
//    charger_i2c_read8(0x39, &reg_val);
//    printf("VBOOST_CTRL[0x39]=0x%x\n", reg_val);

    return 0;
}

static int tcpc_i2c_read_reg(uint8_t reg, uint8_t len, uint8_t *dst)
{
    return _i2c_read_reg(reg, len, dst, TCPC_SLAVE_ADDR);
}

static int tcpc_i2c_write_reg(uint8_t reg, uint8_t len, uint8_t *src)
{
    return _i2c_write_reg(reg, len, src, TCPC_SLAVE_ADDR);
}

static int tcpc_i2c_write8(uint8_t reg, uint8_t data)
{
    int ret;

    ret =  tcpc_i2c_write_reg(reg, 1, &data);
    if (ret) {
        printf("pd i2c write8 Failed: reg=%02X, ret=%d\n", reg, ret);
        return ret;
    }

    return 0;
}

static int tcpc_i2c_write16(uint8_t reg, uint16_t data)
{
    int ret;
    ret = tcpc_i2c_write_reg(reg, 2, (uint8_t *)&data);
    if (ret) {
        printf("pd i2c write16 Failed: reg=%02X, ret=%d\n", reg, ret);
        return ret;
    }

    return 0;
}

static int tcpc_i2c_read8(uint8_t reg, uint8_t *data)
{
    int ret;

    ret = tcpc_i2c_read_reg(reg, 1, data);
    if (ret) {
        printf("pd i2c read8 Failed: reg=%02X, ret=%d\n", reg, ret);
        return ret;
    }

    return 0;
}

static int tcpc_i2c_read16(uint8_t reg, uint16_t *data)
{
    int ret;

    ret = tcpc_i2c_read_reg(reg, 2, (uint8_t *)data);
    if (ret) {
        printf("pd i2c read16 Failed: reg=%02X, ret=%d\n", reg, ret);
        return ret;
    }

    return 0;
}

static int tcpc_i2c_update_bits(uint8_t reg, uint8_t mask, uint8_t val)
{
    uint8_t tmp;
    int ret;

    ret = tcpc_i2c_read8(reg, &tmp);
    if (ret)
        return ret;

    tmp &= ~mask;
    tmp |= val & mask;

    return tcpc_i2c_write8(reg, tmp);
}

int tcpc_command(uint8_t cmd)
{
    return tcpc_i2c_write8(TCPC_REG_COMMAND, cmd);
}

int tcpc_software_reset(void)
{
    int ret;
    uint8_t tmp;

    ret = tcpc_i2c_read8(SGMICRO_REG_SWRESET, &tmp);
    if (ret)
        return ret;

    tmp |= SGMICRO_SOFT_RESET;
    return tcpc_i2c_write8(SGMICRO_REG_SWRESET, tmp);
}

int tcpc_alert_status_clear(uint32_t status)
{
    int ret;
    uint16_t status1, status2; // W1C

    /* clear SG INT before clear vendor_defined_alert */
    status2 = (uint16_t)(status >> 16);
    if (status2) {
        ret = tcpc_i2c_write8(SGMICRO_INT, status2);
        if (ret < 0)
            return ret;
    }

    status1 = (uint16_t) status;
    if (status1) {
        ret = tcpc_i2c_write16(TCPC_REG_ALERT, status1);
        if (ret < 0)
            return ret;
    }

    return 0;
}

static int tcpc_init_status_mask(void)
{
    uint8_t mask;
    uint16_t alert_mask;

    // init alert mask
    alert_mask = TCPC_ALERT_CC_STATUS | TCPC_ALERT_POWER_STATUS \
               | TCPC_ALERT_FAULT | TCPC_ALERT_VENDOR_DEFINED;
#ifdef CONFIG_USBPD_SUPPORT
    // MCU Tx use polling instead of INT
    alert_mask |= TCPC_ALERT_TX_SUCCESS \
                | TCPC_ALERT_TX_DISCARDED \
                | TCPC_ALERT_TX_FAILED \
                | TCPC_ALERT_RX_HARD_RST \
                | TCPC_ALERT_RX_STATUS \
                | TCPC_ALERT_RX_OVERFLOW;
#endif
    tcpc_i2c_write16(TCPC_REG_ALERT_MASK, alert_mask);

    // init sgm vendor mask
    mask = SGMICRO_INT_CC_OVP | SGMICRO_INT_VSAFE0V;
    tcpc_i2c_write8(SGMICRO_INT_MASK, mask);

    // init power mask
    mask = TCPC_POWER_STATUS_VBUS_PRES;
    tcpc_i2c_write8(TCPC_REG_POWER_STATUS_MASK, mask);

    // init fault mask
    mask = TCPC_FAULT_STATUS_VCONN_OC | TCPC_FAULT_STATUS_I2C_ERROR;
    tcpc_i2c_write8(TCPC_REG_FAULT_STATUS_MASK, mask);

    return 0;
}

int tcpc_get_vbus_status(uint8_t *vbus)
{
    int ret;
    uint8_t pwr_sts, sgm_sts;

    ret = tcpc_i2c_read8(TCPC_REG_POWER_STATUS, &pwr_sts);
    if (ret)
        return ret;

    ret = tcpc_i2c_read8(SGMICRO_STATUS, &sgm_sts);
    if (ret)
        return ret;

    if (pwr_sts & TCPC_POWER_STATUS_VBUS_PRES) {
        *vbus = TYPEC_VBUS_VALID;
    } else {
        if (sgm_sts & SGMICRO_VSAFE0V) {
            *vbus = TYPEC_VBUS_VSAFE0V;
        } else {
            *vbus = TYPEC_VBUS_INVALID;
        }
    }

    return 0;
}

int tcpc_get_cc_status(uint8_t *cc1, uint8_t *cc2)
{
    int ret;
    uint8_t cc_sts, role_ctrl, tcpc_ctrl, cc_role;
    bool act_as_drp, act_as_src, cc1_as_comm;

    ret = tcpc_i2c_read8(TCPC_REG_CC_STATUS, &cc_sts);
    if (ret)
        return ret;

    if (cc_sts & TCPC_CC_STATUS_LOOKING4CONN) {
        *cc1 = TYPEC_CC_DRP_TOGGLE;
        *cc2 = TYPEC_CC_DRP_TOGGLE;
        return 0;
    }

    ret = tcpc_i2c_read8(TCPC_REG_ROLE_CTRL, &role_ctrl);
    if (ret)
        return ret;

    act_as_drp = !!(role_ctrl & TCPC_ROLE_CTRL_DRP);
    if (act_as_drp) {
        /* Refer to ConnectResult */
        act_as_src = !(cc_sts & TCPC_CC_STATUS_CONNRESULT);
    } else {
        /* Refer to ROLE_CONTROL.CC */
        ret = tcpc_i2c_read8(TCPC_REG_TCPC_CTRL, &tcpc_ctrl);
        if (ret)
            return ret;

        /* Determine typec orientation */
        cc1_as_comm = !(tcpc_ctrl & TCPC_TCPC_CTRL_PLUG_ORIENT);
        if (cc1_as_comm) {
            cc_role = (role_ctrl & TCPC_ROLE_CTRL_CC1) >> TCPC_ROLE_CTRL_CC1_SHIFT;
        } else {
            cc_role = (role_ctrl & TCPC_ROLE_CTRL_CC2) >> TCPC_ROLE_CTRL_CC2_SHIFT;
        }

        /* Determine typec role */
        if (cc_role == TCPC_ROLE_CTRL_CC_RP) {
            act_as_src = true;
        } else {
            act_as_src = false;
        }
    }

    /* Determine CC1 and CC2 state */
    if (act_as_src) {
        *cc1 = TYPEC_CC_SRC_OPEN + ((cc_sts & TCPC_CC_STATUS_CC1) >> TCPC_CC_STATUS_CC1_SHIFT);
        *cc2 = TYPEC_CC_SRC_OPEN + ((cc_sts & TCPC_CC_STATUS_CC2) >> TCPC_CC_STATUS_CC2_SHIFT);
    } else {
        *cc1 = TYPEC_CC_SNK_OPEN + ((cc_sts & TCPC_CC_STATUS_CC1) >> TCPC_CC_STATUS_CC1_SHIFT);
        *cc2 = TYPEC_CC_SNK_OPEN + ((cc_sts & TCPC_CC_STATUS_CC2) >> TCPC_CC_STATUS_CC2_SHIFT);
    }

    return 0;
}

int tcpc_get_alert_status(uint32_t *alert_sts)
{
    int ret;
    uint16_t std_alert;
    uint8_t vdr_alert;

    ret = tcpc_i2c_read16(TCPC_REG_ALERT, &std_alert);
    if (ret)
        return ret;

    ret = tcpc_i2c_read8(SGMICRO_INT, &vdr_alert);
    if (ret)
        return ret;

    *alert_sts = (vdr_alert << 16) | std_alert;
    return 0;
}

int tcpc_get_tx_result(uint8_t *tx_sts) {
    int ret;

    ret = tcpc_i2c_read8(TCPC_REG_ALERT, tx_sts);
    if (ret)
        return ret;

    /* Tx discarded may be replaced by Rx received  */
    *tx_sts &= TCPC_ALERT_TX_FAILED | TCPC_ALERT_TX_DISCARDED | TCPC_ALERT_TX_SUCCESS | TCPC_ALERT_RX_STATUS;

    return 0;
}

int tcpc_set_cc(uint8_t cc1, uint8_t cc2)
{
    int ret;
    uint8_t role_ctrl;

    ret = tcpc_i2c_read8(TCPC_REG_ROLE_CTRL, &role_ctrl);
    if (ret)
        return ret;

    role_ctrl &= ~(TCPC_ROLE_CTRL_CC2 | TCPC_ROLE_CTRL_CC1 | TCPC_ROLE_CTRL_DRP);
    role_ctrl |= (cc1 << TCPC_ROLE_CTRL_CC1_SHIFT) & TCPC_ROLE_CTRL_CC1;
    role_ctrl |= (cc2 << TCPC_ROLE_CTRL_CC2_SHIFT) & TCPC_ROLE_CTRL_CC2;

    return tcpc_i2c_write8(TCPC_REG_ROLE_CTRL, role_ctrl);
}

int tcpc_set_local_rp_level(uint8_t cc)
{
    return tcpc_i2c_update_bits(TCPC_REG_ROLE_CTRL, TCPC_ROLE_CTRL_RP_VALUE, cc << 4);
}

int tcpc_set_orient(uint8_t polarity)
{
    return tcpc_i2c_update_bits(TCPC_REG_TCPC_CTRL, TCPC_TCPC_CTRL_PLUG_ORIENT, \
                                polarity ? TCPC_TCPC_CTRL_PLUG_ORIENT : 0);
}

int tcpc_start_drp_toggle(void)
{
    int ret;

    /* TODO: shall check CC1/CC2 is Rp/Rp or Rd/Rd */

    ret = tcpc_i2c_update_bits(TCPC_REG_ROLE_CTRL, TCPC_ROLE_CTRL_DRP, TCPC_ROLE_CTRL_DRP);
    if (ret)
        return ret;

    return tcpc_command(TCPC_COMMAND_LOOK4CONNECTION);
}

int tcpc_set_vconn(bool enable)
{
    return tcpc_i2c_update_bits(TCPC_REG_POWER_CTRL, TCPC_POWER_CTRL_EN_VCONN, 
                            enable ? TCPC_POWER_CTRL_EN_VCONN : 0);
}

int tcpc_get_remote_rplvl(uint8_t *rp_level, uint8_t polarity)
{
    int ret;
    uint8_t cc1, cc2;

    ret = tcpc_get_cc_status(&cc1, &cc2);
    if (ret)
        return ret;

    if (polarity) {
        if (cc2 > TYPEC_CC_SNK_OPEN) {
            *rp_level = TYPEC_REMOTE_RP_DEFAULT + (cc2 - TYPEC_CC_SNK_DEFAULT);
            ret = 0;
        } else {
            ret = -1;
        }
    } else {
        if (cc1 > TYPEC_CC_SNK_OPEN) {
            *rp_level = TYPEC_REMOTE_RP_DEFAULT + (cc1 - TYPEC_CC_SNK_DEFAULT);
            ret = 0;
        } else {
            ret = -1;
        }
    }

    return ret;
}

int tcpc_get_rx_message(uint8_t *frame_type, uint16_t *msg_hdr, uint32_t *payload)
{
    int ret;
    uint8_t buf[4];
    uint8_t cnt;

    ret = tcpc_i2c_read_reg(TCPC_REG_RX_BYTE_CNT, 4, buf);
    if (ret)
        return ret;

    cnt = buf[0];
    *frame_type = buf[1] & TCPC_RECEIVED_SOP_MESSAGE;
    *msg_hdr = *(uint16_t *)&buf[2];

    if (cnt > 3) {
        cnt -= 3; // minus FRAME_TYPE and Message Header
        ret = tcpc_i2c_read_reg(TCPC_REG_RX_DATA, cnt, (uint8_t *)payload);
        if (ret)
            return ret;
    }

    return 0;
}

int tcpc_transmit(uint8_t frame_type, uint16_t header, uint8_t retry, const uint32_t *data)
{
    int ret;
    uint8_t data_cnt;
    struct tcpc_transmit_data tx_data;

    if (frame_type < FRAME_HARD_RESET) {
        data_cnt = PD_MSG_HEADER_CNT(header) * 4;
        tx_data.cnt = data_cnt + 2; // include 16-bit message header
        tx_data.msg_header = header;
        if (data_cnt > 0)
            memcpy(tx_data.data, (uint8_t *)data, data_cnt);

        ret = tcpc_i2c_write_reg(TCPC_REG_TX_BYTE_CNT, tx_data.cnt + 1, (uint8_t *)&tx_data);
        if (ret)
            return ret;
    }

    return tcpc_i2c_write8(TCPC_REG_TRANSMIT, TCPC_TRANSMIT_SET(retry, frame_type));
}

int tcpc_set_bist_test_mode(bool enable)
{
    uint8_t data;
    int ret;

    ret = tcpc_i2c_read8(TCPC_REG_TCPC_CTRL, &data);
    if (ret < 0)
        return ret;

    data &= ~TCPC_TCPC_CTRL_BIST_TEST_MODE;
    data |= enable ? TCPC_TCPC_CTRL_BIST_TEST_MODE : 0;

    /* clear rx buffer before exiting BIST mode, along with sop rx enabled */
    if (!enable) {
        tcpc_i2c_write8(TCPC_REG_ALERT, TCPC_ALERT_RX_HARD_RST | TCPC_ALERT_RX_STATUS);
    }

    /* mask rx int => need confirm if needed? */
    tcpc_i2c_write8(TCPC_REG_ALERT_MASK, enable ? 0x7B : 0x7F);

	return tcpc_i2c_write8(TCPC_REG_TCPC_CTRL, data);
}

int tcpc_set_rx_enable(uint8_t enable)
{
    return tcpc_i2c_write8(TCPC_REG_RX_DETECT, enable);
}

int tcpc_set_msg_header(uint8_t power_role, uint8_t data_role)
{
    /**
     * Refer to 6.2.1.1.5 Specification Revision:
     * The sender of GoodCRC shall set Spec Revision to 01b when responding to Message of Spec Revision 01b.
     * And may set Spec Revision to 01b or 10b for that of Spec Revision 10b.
     * So always set HW GoodCRC revision to 01b here.
     */
    uint8_t msg_hdr = TCPC_MSG_HDR_INFO_SET(0, PD_REVISION_20, data_role, power_role);

    return tcpc_i2c_write8(TCPC_REG_MSG_HDR_INFO, msg_hdr);
}

void delay_1ms(uint32_t n);
int tcpc_hw_init(struct typec_port_info *port_info)
{
    int ret;

    /* check revision */
    ret = tcpc_i2c_read16(TCPC_REG_VID, &port_info->vid);
    if (ret)
        return ret;
    ret = tcpc_i2c_read16(TCPC_REG_PID, &port_info->pid);
    if (ret)
        return ret;
    ret = tcpc_i2c_read16(TCPC_REG_DID, &port_info->did);
    if (ret)
        return ret;
    printf("%s read device, vid=0x%04x, pid=0x%04x, did=0x%04x\n", __func__, 
            port_info->vid, port_info->pid, port_info->did);

    ret = tcpc_software_reset();
    if (ret)
        return ret;
    delay_1ms(1);

    /* DRP = 0, RpVal = 0 (Default), Rd, Rd */
    tcpc_i2c_write8(TCPC_REG_ROLE_CTRL, TCPC_ROLE_CTRL_SET(0, port_info->local_rplvl, 
                                        TCPC_ROLE_CTRL_CC_RD, TCPC_ROLE_CTRL_CC_RD));

    /**
     * DRP Toggle Cycle : 51.2 + 6.4*val ms
     * DRP Duty Ctrl : 330 / 1024
     */
    tcpc_i2c_write8(SGMICRO_REG_DRP_TOGGLE_CYCLE, 4);
    tcpc_i2c_write16(SGMICRO_REG_DRP_DUTY_CTRL0, 330);

    /* Unmask INT and clear status */
    tcpc_alert_status_clear(0xffffffff);
    tcpc_init_status_mask();

    /* CK_300K from 320K, Shutdown off, Enable Ext, AUTOIDLE enable, TIMEOUT = 32ms */
    tcpc_i2c_write8(SGMICRO_CTRL2, SGMICRO_CK_300K_SEL | SGMICRO_SHUTDOWN_OFF \
        | SGMICRO_ENEXTMSG | SGMICRO_AUTOIDLE_EN \
        | (2 & SGMICRO_AUTOIDLE_TIMEOUT));
    delay_1ms(10);

    return 0;
}

void dump_tcpc_registers(void)
{
    uint8_t i, data;

    for (i = 0; i < 0x30; i++) {
        tcpc_i2c_read8(i, &data);
        printf("REG[0x%02X]=0x%02X ", i, data);
        if ((i + 1) % 16 == 0)
            printf("\n");
    }

    for (i = 0x90; i < 0xA6; i++) {
        tcpc_i2c_read8(i, &data);
        printf("REG[0x%02X]=0x%02X ", i, data);
        if ((i + 1) % 16 == 0)
            printf("\n");
    }

    printf("\n");
}
